#include "distributedMapping.h"

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
	class distributedMapping: publish visualization msg
	为了实现分布式映射功能而创建的。"publish visualization msg"方法的目的可能是将可视化消息进行发布，以供其他组件或系统使用。
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void distributedMapping::globalMapThread()
{
	ros::Rate rate(1.0/map_publish_interval_); // update global map per 4s (default)；更新全局地图的频率，单位是秒。

	while(ros::ok())
	{
		rate.sleep();

		publishGlobalMap(); // global map visualization；全局地图可视化

		publishLoopClosureConstraint(); // loop closure visualization触发循环闭合可视化操作，生成和发布循环闭合约束的可视化结果，以便用户可以查看和分析闭合环的情况。
		
	}
}

//全局地图可视化
void distributedMapping::publishGlobalMap()
{
	// getNumSubscribers()是一个函数，它返回订阅该对象的客户端数量。
	// initial_values分布式地图为空
	if(pub_global_map.getNumSubscribers() == 0 || initial_values->empty() == true)
	{
		return;
	}

	// copy the poses and change to cloud type
	//复制位姿分别给poses_3d_cloud_copy为xyz，给poses_6d_cloud_copy为xyzrpy
	Values poses_initial_guess_copy = *initial_values;
	pcl::PointCloud<PointPose3D>::Ptr poses_3d_cloud_copy(new pcl::PointCloud<PointPose3D>());
	pcl::PointCloud<PointPose6D>::Ptr poses_6d_cloud_copy(new pcl::PointCloud<PointPose6D>());
	//引用key_value 遍历poses_initial_guess_copy
	for(const Values::ConstKeyValuePair &key_value: poses_initial_guess_copy)
	{
		Symbol key = key_value.key;//获取键值对中的键
		Pose3 pose = poses_initial_guess_copy.at<Pose3>(key);

		PointPose3D pose_3d;//每个元素都是pose_3d
		pose_3d.x = pose.translation().x();
		pose_3d.y = pose.translation().y();
		pose_3d.z = pose.translation().z();
		pose_3d.intensity = key.index();//将insensity的值设置为索引，用于确定key在集合中的位置

		PointPose6D pose_6d;
		pose_6d.x = pose_3d.x;
		pose_6d.y = pose_3d.y;
		pose_6d.z = pose_3d.z;
		pose_6d.intensity = pose_3d.intensity;
		pose_6d.roll = pose.rotation().roll();
		pose_6d.pitch = pose.rotation().pitch();
		pose_6d.yaw = pose.rotation().yaw();

		poses_3d_cloud_copy->push_back(pose_3d);
		poses_6d_cloud_copy->push_back(pose_6d);
	}

	// find the closest history key frame寻找最近历史关键帧
	std::vector<int> indices;//索引
	std::vector<float> distances;//距离
	kdtree_history_keyposes->setInputCloud(poses_3d_cloud_copy);//将poses_3d_cloud_copy作为输入传递给kdtree_history_keyposes的kd树
	/*
	poses_3d_cloud_copy->back()表示根据poses_3d_cloud_copy的最后一个元素进行搜索
	global_map_visualization_radius_搜索半径
	搜索结果以两个数组 indices 和 distances 返回。indices 数组存储了匹配点在 poses_3d_cloud_copy 中的索引，而 distances 数组则存储了对应的匹配距离。
	*/
	kdtree_history_keyposes->radiusSearch(poses_3d_cloud_copy->back(),
		global_map_visualization_radius_, indices, distances, 0);//根据半径搜索，

	// extract visualized key frames；定义两个指针
	pcl::PointCloud<PointPose3D>::Ptr global_map_keyframes(new pcl::PointCloud<PointPose3D>());
	pcl::PointCloud<PointPose3D>::Ptr global_map_keyframes_ds(new pcl::PointCloud<PointPose3D>());
	/*遍历索引indices
	transformPointCloud()，将 robots[id_].keyframe_cloud_array[pose_6d_tmp.intensity] 通
	过 pose_6d_tmp 对应的'变换矩阵'进行变换并返回一个新的点云数据集，
	然后将该点云数据集与 *global_map_keyframes 相加。
	*/
	for (int i = 0; i < (int)indices.size(); ++i)
	{

		// 找到输入的odom值
		


		PointPose6D pose_6d_tmp = poses_6d_cloud_copy->points[indices[i]];
		*global_map_keyframes += *transformPointCloud(robots[id_].keyframe_cloud_array[pose_6d_tmp.intensity],
			&pose_6d_tmp);
	}

	// downsample visualized points降采样可视化点
	pcl::VoxelGrid<PointPose3D> downsample_filter_for_global_map; // for global map visualization
	downsample_filter_for_global_map.setLeafSize(map_leaf_size_, map_leaf_size_, map_leaf_size_);
	downsample_filter_for_global_map.setInputCloud(global_map_keyframes);
	downsample_filter_for_global_map.filter(*global_map_keyframes_ds);

	// publish global map转换成ros标准点云格式发布
	sensor_msgs::PointCloud2 global_map_msg;
	pcl::toROSMsg(*global_map_keyframes_ds, global_map_msg);
	global_map_msg.header.stamp = robots[id_].time_cloud_input_stamp;
	global_map_msg.header.frame_id = world_frame_;
	pub_global_map.publish(global_map_msg);
}

//根据输入的节点和边的信息，生成可视化循化闭合约束标记
void distributedMapping::publishLoopClosureConstraint()
{
	// 检查loop_indexs是否为空
	if(loop_indexs.empty())
	{
		return;
	}

	// loop nodes
	visualization_msgs::Marker nodes;//创建一个可视化标记器对象
	nodes.header.frame_id = world_frame_;//id
	nodes.header.stamp = ros::Time::now();//时间戳
	nodes.action = visualization_msgs::Marker::ADD;//visualization_msgs::Marker是一种消息类型，ADD表示增加
	nodes.type = visualization_msgs::Marker::SPHERE_LIST;//设置nodes标记器的类型为球体列表(sphere list)，表示将以球体的形式展示一系列节点
	nodes.ns = "loop_nodes";//命名
	nodes.id = 0;
	nodes.pose.orientation.w = 1;
	nodes.scale.x = 0.3; nodes.scale.y = 0.3; nodes.scale.z = 0.3; //缩放(scale)参数
	nodes.color.r = 0; nodes.color.g = 0.8; nodes.color.b = 1;
	nodes.color.a = 1;

	// loop edges对边的定义，类似
	visualization_msgs::Marker constraints;
	constraints.header.frame_id = world_frame_;
	constraints.header.stamp = ros::Time::now();
	constraints.action = visualization_msgs::Marker::ADD;
	constraints.type = visualization_msgs::Marker::LINE_LIST;
	constraints.ns = "loop_constraints";
	constraints.id = 1;
	constraints.pose.orientation.w = 1;
	constraints.scale.x = 0.1;
	constraints.color.r = 0.9; constraints.color.g = 0.9; constraints.color.b = 0;
	constraints.color.a = 1;

	// check all accepted loop closure
	Pose3 pose;
	int index0, index1;
	//将当前迭代器指向的loop_indexs的键值分别赋给index0和index1。
	for(auto it = loop_indexs.begin(); it != loop_indexs.end(); ++it)
    {
		index0 = it->first;
		index1 = it->second;

		geometry_msgs::Point p;//定义点的坐标
		pose = initial_values->at<Pose3>(Symbol('a'+id_, index0));
		p.x = pose.translation().x();
		p.y = pose.translation().y();
		p.z = pose.translation().z();
		nodes.points.push_back(p);
		constraints.points.push_back(p);
		pose = initial_values->at<Pose3>(Symbol('a'+id_, index1));
		p.x = pose.translation().x();
		p.y = pose.translation().y();
		p.z = pose.translation().z();
		nodes.points.push_back(p);
		constraints.points.push_back(p);
	}

	// publish loop closure markers
	visualization_msgs::MarkerArray markers_array;//存储闭环检测标记的数组
	//添加nodes闭环节点和constraints闭环约束标记，使用markers_array发布
	markers_array.markers.push_back(nodes);
	markers_array.markers.push_back(constraints);
	pub_loop_closure_constraints.publish(markers_array);
}